#include <ros/ros.h>
#include <image_transport/image_transport.h>
//#include </home/nvidia/opencv-3.2.0/include/opencv2/highgui/highgui.hpp>
#include </home/nvidia/opencv-4.5.4/include/opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>
#include <iostream>

#include "cuda_utils.h"
#include "logging.h"
#include "utils.h"
#include "preprocess.h"
#include "postprocess.h"
#include "model.h"

#include <chrono>
//#include <cmath>
#include <typeinfo>

//#include <std_msgs/Float32MultiArray.h>
#include <swarm_msgs/BoundingBoxes.h>
#include <swarm_msgs/BoundingBox.h>

using namespace nvinfer1;

static Logger gLogger;
const static int kOutputSize = kMaxNumOutputBbox * sizeof(Detection) / sizeof(float) + 1;
std::string engine_name = "/home/nvidia/yolo_weight/jr.engine";
float gd = 0.0f, gw = 0.0f;
std::string img_dir = "../images";
IRuntime* runtime = nullptr;
ICudaEngine* engine = nullptr;
IExecutionContext* context = nullptr;
cudaStream_t stream;
float* gpu_buffers[2];  
float* cpu_output_buffer = nullptr;

ros::Publisher pub_circle; //Red Circle bian/circle
ros::Publisher pub_human; //JR bian/humans
ros::Publisher pub_ballon; //Pipline bian/pipline
ros::Publisher pub_stair; //Stair bian/stair
ros::Publisher pub_ammon_box; 
ros::Publisher pub_water; 
ros::Publisher pub_oil; 
ros::Publisher pub_no_smoking; 


void prepare_buffers(ICudaEngine* engine, float** gpu_input_buffer, float** gpu_output_buffer, float** cpu_output_buffer) {
  assert(engine->getNbBindings() == 2);
  // In order to bind the buffers, we need to know the names of the input and output tensors.
  // Note that indices are guaranteed to be less than IEngine::getNbBindings()
  const int inputIndex = engine->getBindingIndex(kInputTensorName);
  const int outputIndex = engine->getBindingIndex(kOutputTensorName);
  assert(inputIndex == 0);
  assert(outputIndex == 1);
  // Create GPU buffers on device
  CUDA_CHECK(cudaMalloc((void**)gpu_input_buffer, kBatchSize * 3 * kInputH * kInputW * sizeof(float)));
  CUDA_CHECK(cudaMalloc((void**)gpu_output_buffer, kBatchSize * kOutputSize * sizeof(float)));

  *cpu_output_buffer = new float[kBatchSize * kOutputSize];
}

void infer(IExecutionContext& context, cudaStream_t& stream, void** gpu_buffers, float* output, int batchsize) {
  context.enqueue(batchsize, gpu_buffers, stream, nullptr);
  CUDA_CHECK(cudaMemcpyAsync(output, gpu_buffers[1], batchsize * kOutputSize * sizeof(float), cudaMemcpyDeviceToHost, stream));
  cudaStreamSynchronize(stream);
}

void deserialize_engine(std::string& engine_name, IRuntime** runtime, ICudaEngine** engine, IExecutionContext** context) {
  std::ifstream file(engine_name, std::ios::binary);
  if (!file.good()) {
    std::cerr << "read " << engine_name << " error!" << std::endl;
    assert(false);
  }
  size_t size = 0;
  file.seekg(0, file.end);
  size = file.tellg();
  file.seekg(0, file.beg);
  char* serialized_engine = new char[size];
  assert(serialized_engine);
  file.read(serialized_engine, size);
  file.close();

  *runtime = createInferRuntime(gLogger);
  assert(*runtime);
  *engine = (*runtime)->deserializeCudaEngine(serialized_engine, size);
  assert(*engine);
  *context = (*engine)->createExecutionContext();
  assert(*context);
  delete[] serialized_engine;
}


void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
 try
  { // Get image
    auto cv_ptr = cv_bridge::toCvShare(msg, "bgr8");
    std::vector<cv::Mat> img_batch;
    cv::Mat img = cv_ptr->image;
    img_batch.push_back(img);
    
    //Preprocess
    cuda_batch_preprocess(img_batch, gpu_buffers[0], kInputW, kInputH, stream);

    // Run inference
    auto start = std::chrono::system_clock::now();
    infer(*context, stream, (void**)gpu_buffers, cpu_output_buffer, kBatchSize);
    auto end = std::chrono::system_clock::now();
    //std::cout << "inference time: " << std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count() << "ms" << std::endl;

    // NMS
    std::vector<std::vector<Detection>> res_batch;
    batch_nms(res_batch, cpu_output_buffer, img_batch.size(), kOutputSize, kConfThresh, kNmsThresh);

    // Draw bounding boxes
    auto& res = res_batch[0];
    for (size_t j = 0; j < res.size(); j++) {

      //cv::Rect r = get_rect(img, res[j].bbox);
      //cv::rectangle(img, r, cv::Scalar(0x27, 0xC1, 0x36), 2);
      std::cout<<"-->xmin"<<(int)(res[j].bbox[0]-0.5*res[j].bbox[2])/640.0*1024<<std::endl; 
      std::cout<<"-->ymin"<<(int)(res[j].bbox[1]-0.5*res[j].bbox[3])/640.0*768<<std::endl;
      std::cout<<"-->xmax"<<(int)(res[j].bbox[0]+0.5*res[j].bbox[2])/640.0*1024<<std::endl;
      std::cout<<"-->ymax"<<(int)(res[j].bbox[1]+0.5*res[j].bbox[3])/640.0*768<<std::endl;
      std::cout<<"--->class id: "<<std::to_string((int)res[j].class_id)<<std::endl;
      cv::Rect r = get_rect(img, res[j].bbox);
      cv::rectangle(img, r, cv::Scalar(0x27, 0xC1, 0x36), 2);


      std::cout<<std::to_string((int)res[j].class_id)<<std::endl;
      switch((int)res[j].class_id){
        case 0: //circle
          cv::putText(img, "circle"+std::to_string(res[j].conf), cv::Point(r.x, r.y - 1), cv::FONT_HERSHEY_PLAIN, 1.2, cv::Scalar(0xFF, 0xFF, 0xFF), 2);
          //std::cout<<"circle"<<std::endl;
          //std::cout<<res[j].conf<<std::endl;
          break;
        case 1: //human
          cv::putText(img, "human"+std::to_string(res[j].conf), cv::Point(r.x, r.y - 1), cv::FONT_HERSHEY_PLAIN, 1.2, cv::Scalar(0xFF, 0xFF, 0xFF), 2);
          //std::cout<<"human"<<std::endl;
          //std::cout<<res[j].conf<<std::endl;
          break;
        case 2: //ballon
          cv::putText(img, "ballon"+std::to_string(res[j].conf), cv::Point(r.x, r.y - 1), cv::FONT_HERSHEY_PLAIN, 1.2, cv::Scalar(0xFF, 0xFF, 0xFF), 2);
          //std::cout<<"ballon"<<std::endl;
          //std::cout<<res[j].conf<<std::endl;
          break;
        case 3: //stair
          cv::putText(img, "stair"+std::to_string(res[j].conf), cv::Point(r.x, r.y - 1), cv::FONT_HERSHEY_PLAIN, 1.2, cv::Scalar(0xFF, 0xFF, 0xFF), 2);
          //std::cout<<"stair"<<std::endl;
          //std::cout<<res[j].conf<<std::endl;
          break;
        case 4: //ammon-box
          cv::putText(img, "ammon-box"+std::to_string(res[j].conf), cv::Point(r.x, r.y - 1), cv::FONT_HERSHEY_PLAIN, 1.2, cv::Scalar(0xFF, 0xFF, 0xFF), 2);
          //std::cout<<"ammon-box"<<std::endl;
          //std::cout<<res[j].conf<<std::endl;
          break;
        case 5: //water
          cv::putText(img, "water"+std::to_string(res[j].conf), cv::Point(r.x, r.y - 1), cv::FONT_HERSHEY_PLAIN, 1.2, cv::Scalar(0xFF, 0xFF, 0xFF), 2);
          //std::cout<<"water"<<std::endl;
          //std::cout<<res[j].conf<<std::endl;
          break;
        case 6: //oil
          cv::putText(img, "oil"+std::to_string(res[j].conf), cv::Point(r.x, r.y - 1), cv::FONT_HERSHEY_PLAIN, 1.2, cv::Scalar(0xFF, 0xFF, 0xFF), 2);
          //std::cout<<"oil"<<std::endl;
          //std::cout<<res[j].conf<<std::endl;
          break;
        case 7: //no-smoking
          cv::putText(img, "no-smoking"+std::to_string(res[j].conf), cv::Point(r.x, r.y - 1), cv::FONT_HERSHEY_PLAIN, 1.2, cv::Scalar(0xFF, 0xFF, 0xFF), 2);
          //std::cout<<"no-smoking"<<std::endl;
          //std::cout<<res[j].conf<<std::endl;
          break;
      }

    }
  
    //img
    cv::imshow("view", img);
    cv::waitKey(1);
    swarm_msgs::BoundingBoxes circle_boxes;
    swarm_msgs::BoundingBoxes human_boxes;
    swarm_msgs::BoundingBoxes ballon_boxes;
    swarm_msgs::BoundingBoxes stair_boxes;
    swarm_msgs::BoundingBoxes ammon_box_boxes;
    swarm_msgs::BoundingBoxes water_boxes;
    swarm_msgs::BoundingBoxes oil_boxes;
    swarm_msgs::BoundingBoxes no_smoking_boxes;
    
    for(int i=0;i<res_batch[0].size();i++){
      //std::cout <<"--->class_id: "<< res_batch[0][i].class_id << std::endl;
      swarm_msgs::BoundingBox msg_BoundingBox;
      msg_BoundingBox.probability=res_batch[0][i].conf;
      msg_BoundingBox.xmin=(res_batch[0][i].bbox[0]-0.5*res_batch[0][i].bbox[2])/640.0*1024;
      msg_BoundingBox.xmax=(res_batch[0][i].bbox[0]+0.5*res_batch[0][i].bbox[2])/640.0*1024;
      msg_BoundingBox.ymin=(res_batch[0][i].bbox[1]-0.5*res_batch[0][i].bbox[3])/640.0*768;
      msg_BoundingBox.ymax=(res_batch[0][i].bbox[1]+0.5*res_batch[0][i].bbox[3])/640.0*768;

      
      switch((int)res_batch[0][i].class_id){
        case 0: //circle
          if(res_batch[0][i].conf>.70){
            circle_boxes.bounding_boxes.push_back(msg_BoundingBox);
          }   
          break;
        case 1: //human
          if(res_batch[0][i].conf>.70){
            human_boxes.bounding_boxes.push_back(msg_BoundingBox);
          }
          break;
        case 2: //ballon
          if(res_batch[0][i].conf>.75){
            ballon_boxes.bounding_boxes.push_back(msg_BoundingBox);
          }
          break;
        case 3: //stair
          if(res_batch[0][i].conf>.70){
            stair_boxes.bounding_boxes.push_back(msg_BoundingBox);
          }
          break;
        case 4: //ammon-box
          if(res_batch[0][i].conf>.70){
            ammon_box_boxes.bounding_boxes.push_back(msg_BoundingBox);
          }
          break;
        case 5: //water
          if(res_batch[0][i].conf>.70){
            water_boxes.bounding_boxes.push_back(msg_BoundingBox);
          }
          break;
        case 6: //oil
          if(res_batch[0][i].conf>.70){
            oil_boxes.bounding_boxes.push_back(msg_BoundingBox);
          }
          break;
        case 7: //no-smoking
          if(res_batch[0][i].conf>.70){
            no_smoking_boxes.bounding_boxes.push_back(msg_BoundingBox);
          }
          break;
      }
    }
    pub_human.publish(human_boxes);
    pub_circle.publish(circle_boxes);
    pub_stair.publish(stair_boxes);
    pub_ballon.publish(ballon_boxes);
    pub_ammon_box.publish(ammon_box_boxes);
    pub_water.publish(water_boxes);
    pub_oil.publish(oil_boxes);
    pub_no_smoking.publish(no_smoking_boxes);
  }
  catch (cv_bridge::Exception& e)
  {
    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
  }

}
 
int main(int argc, char **argv)
{
  cudaSetDevice(kGpuId);

  // Deserialize the engine from file
  deserialize_engine(engine_name, &runtime, &engine, &context);
  CUDA_CHECK(cudaStreamCreate(&stream));
  // Init CUDA preprocessing
  cuda_preprocess_init(kMaxInputImageSize);
  // Prepare cpu and gpu buffers
  prepare_buffers(engine, &gpu_buffers[0], &gpu_buffers[1], &cpu_output_buffer);

  ros::init(argc, argv, "image_listener");
  ros::NodeHandle nh;

  //cv::namedWindow("view");
  //cv::startWindowThread();
  image_transport::ImageTransport it(nh);
  pub_human = nh.advertise<swarm_msgs::BoundingBoxes>("/bian/humans", 1);
  pub_circle = nh.advertise<swarm_msgs::BoundingBoxes>("/bian/circle", 1);
  pub_stair = nh.advertise<swarm_msgs::BoundingBoxes>("/bian/stair", 1);
  pub_ballon = nh.advertise<swarm_msgs::BoundingBoxes>("/bian/ballon", 1);
  pub_ammon_box = nh.advertise<swarm_msgs::BoundingBoxes>("/bian/ammon_box", 1);
  pub_water = nh.advertise<swarm_msgs::BoundingBoxes>("/bian/water", 1);
  pub_oil = nh.advertise<swarm_msgs::BoundingBoxes>("/bian/oil", 1);
  pub_no_smoking = nh.advertise<swarm_msgs::BoundingBoxes>("/bian/no_smoking", 1);
  image_transport::Subscriber sub = it.subscribe("/usb_cam/image_raw", 1, imageCallback);
  //image_transport::Subscriber sub = it.subscribe("/csi_cam/image", 1, imageCallback);
  
  ros::spin();
  //cv::destroyWindow("view");
}

